/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file joystickTranThread.cxx
	\brief class implemenation for joystickTranThread

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <linux/joystick.h>

#include "joystickTranThread.h"

  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
		extern Client * js_client;
  /*<-End Global Variables-->*/

JoystickTranThread::JoystickTranThread(String name) : ThreadedPipe(name)
{
  // Intialize the counter
  clock_gettime(CLOCK_MONOTONIC, &t);
  start = (float)t.tv_sec*1000.0 + (float)t.tv_nsec/1000000.0;

  servo_cmds.channels[0] = 32767;
  servo_cmds.channels[1] = 32767;
  servo_cmds.channels[2] = 0;
  servo_cmds.channels[3] = 32767;

  button_cmds.channels[0] = 0;
  button_cmds.channels[1] = 0;
  button_cmds.channels[2] = 0;
  button_cmds.channels[3] = 0;

  axis_change = 1;
  button_change = 1;
}

void JoystickTranThread::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("joystickTranThread::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(OUTBOARD_RX);
      if(n>0) {
        parse();
      }
    }
  }

  clock_gettime(CLOCK_MONOTONIC, &t);
  end = (float)t.tv_sec*1000.0 + (float)t.tv_nsec/1000000.0;

  if (end-start > DELAY) { // Send out joystick commands every DELAY ms.

		start = end;

		if(js_client != NULL) {
			if (axis_change == 1){
				uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);
				memcpy(dataPtr, &servo_cmds, sizeof(Servos_t));
				CMD_CMD(fifo_msg.data)  = CMD_SERVO;
				CMD_NUM(fifo_msg.data)  = sizeof(Servos_t);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);
				fifo_msg.dest.s_addr = js_client->address[0].s_addr;
				fifo_msg.src.s_addr  = shmPtr->clients.getClient(0)->address[0].s_addr;
				if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
					writeFifo(INBOARD_TX);
					if(verbose)
						printf("(%d,%d,%d,%d)\n",servo_cmds.channels[0],servo_cmds.channels[1],servo_cmds.channels[2],servo_cmds.channels[3]);
					axis_change = 0;
				}
			}
			if (button_change == 1){
				uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);
				memcpy(dataPtr, &button_cmds, sizeof(Buttons_t));
				CMD_CMD(fifo_msg.data)  = CMD_BUTTONS;
				CMD_NUM(fifo_msg.data)  = sizeof(Buttons_t);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.dest.s_addr = js_client->address[0].s_addr;
				fifo_msg.src.s_addr  = shmPtr->clients.getClient(0)->address[0].s_addr;
				fifo_msg.size = CMD_SIZE(fifo_msg.data);
				if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
					writeFifo(INBOARD_TX);
					button_change = 0;
				}
			}
		}
  }
	usleep(10000);
}

// This function takes the joystick commands and converts them to Velocity and turn_rate
void JoystickTranThread::parse()
{

  js_event * joy_ptr = (js_event *)fifo_msg.data;
  switch(joy_ptr->type){
    case AXIS:
			float percent;
			axis_change=1;
      switch(joy_ptr->number){
          case 0:
            percent = (float)(-joy_ptr->value)/41214.0;
						(percent < -0.5) ? percent = -0.5 : percent = percent;
						(percent > 0.5) ? percent = 0.5 : percent = percent;
						servo_cmds.channels[0] = (uint16_t)((percent + 0.5)*65535);
            break;
          case 1:
            percent  = (float)(-joy_ptr->value)/41214.0;
						(percent < -0.5) ? percent = -0.5 : percent = percent;
						(percent > 0.5) ? percent = 0.5 : percent = percent;
						servo_cmds.channels[1] = (uint16_t)((percent + 0.5)*65535);
            break;
          case 2:
            percent  = (float)(-joy_ptr->value)/41214.0;
						(percent < -0.5) ? percent = -0.5 : percent = percent;
						(percent > 0.5) ? percent = 0.5 : percent = percent;
						servo_cmds.channels[2] = (uint16_t)((percent + 0.5)*65535);
            break;
          case 3:
						break;
          case 4:
            percent  = (float)(-joy_ptr->value)/41214.0;
						(percent < -0.5) ? percent = -0.5 : percent = percent;
						(percent > 0.5) ? percent = 0.5 : percent = percent;
						servo_cmds.channels[3] = (uint16_t)((percent + 0.5)*65535);
            break;
          default:
            break;
          }
					//printf("%d (%d,%d,%d,%d)\n",joy_ptr->number,servo_cmds.channels[0],servo_cmds.channels[1],servo_cmds.channels[2],servo_cmds.channels[3]);
      break;
		case BUTTON:
			button_change=1;
      switch(joy_ptr->number){
          case 0:
						button_cmds.channels[0] = joy_ptr->value;
            break;
          case 1:
						button_cmds.channels[1] = joy_ptr->value;
            break;
          case 2:
						button_cmds.channels[2] = joy_ptr->value;
            break;
          case 3:
						button_cmds.channels[3] = joy_ptr->value;
            break;
          default:
            break;
          }
      break;
    default:
      break;
  }
}
